In this section you will control the paddle to move to a desired location. The robot is force controlled. This means that for every time step, you can specify an applied force to the robot's center of mass. Additionally you can specify the an applied angular torque.
The goal is to program the robot to move to a desired location specified by $\vec{x}^* = (x,y,\theta)$ by specifing the velocity at each time step.
We will break this into a few steps.
The following code shows the instructor solution for a simple PD controller. You can modify the initial position and desired position/velocity of the robot to see how it works.
In [ ]:
import tutorial; from tutorial import *
initial_pose = (16, 12,0.0)
desired_pose = (16, 16,3.14/2.)
desired_vel = (0, 0, 0)
play_pd_control_solution(initial_pose, \
desired_pose, desired_vel)
The following code will show a simple example of running the simulator
You should see the robot (paddle) start in the middle of the screen and fall down due to gravity.
Try changing the robot's orientation and rerun the simulation.
In [ ]:
import tutorial; from tutorial import *
initial_pose = (16, 12, 3.14/2.)
result = run_pd_control(initial_pose, controller=None)
Let's rerun our simulation and plot the state of the robot over time.
In [ ]:
import tutorial; reload(tutorial); from tutorial import *
initial_pose = (16, 12, 3.14/2.)
result = run_pd_control(initial_pose, controller=None)
plot(result, "Robot")
Now we are going to move our robot using open loop control. We can apply a force to the center of mass in the x or y direction, and an angular torque about the center of mass.
One of the inputs to the run_pd_control
is currently set to None. In this example we are going to show how to write a controller that gets run at every time step.
The output of the controller is $u_x, u_y, u_th$, which is the amount of force applied in the x direction, in the y direction, and angular torque applied. The force is applied to the robot's center of mass.
In [ ]:
import tutorial; reload(tutorial); from tutorial import *
initial_pose = (16, 12, 3.14/2.)
def openLoopController (time, robot_state):
u_x = 1.0
u_y = 0
u_th = 0
return u_x, u_y, u_th
result = run_pd_control(initial_pose, openLoopController)
plot(result, "Robot")
Suppose we want to move our robot up 4 meters to position (16, 16) from position (16, 12) using our open loop control function. What forces should we apply and for how long? The mass of the robot is 2 kg.
Assuming we apply a constant force $u_y$, the dynamics of the system will be: $$ y(t) = y_0 + \frac{1}{2}(\frac{u_y}{m} - 9.81)t^2 $$
If we assume the force will be applied for 2 seconds only, we can find what constant force to apply:
$$ 16 = 12 + \frac{1}{2}(\frac{u_y}{m} - 9.81)2^2 $$$$ u_y = 23.62 $$Program the robot to move to position (16, 15) using open loop commands. How close can you get?
In [ ]:
import tutorial; reload(tutorial); from tutorial import *
initial_pose = (16, 12,0.0)
constant_force = 23.62
time_applied = 2
def openLoopController (time, robot_state):
u_x = 0
u_y = 0
u_th = 0
# only apply force for time < time_applied
if time < time_applied:
u_y = constant_force
# when the robot is near time_applied print the current y value
if abs(time-time_applied) < 0.1:
print "Time: %.2f, Height: %.2f " % (time, robot_state[1])
return u_x, u_y, u_th
result = run_pd_control(initial_pose, openLoopController)
plot(result, "Robot")
The open loop controller method we used required a lot of effort on the designers part and won't work very well in practice. In this case we knew the robot's mass and could perfectly apply a force in the center of motion.
An alternative method is to use the current state of the robot to determine what force to apply. In this next section you are going to implement a position controller.
The following is an equation for a position controller:
$$u = K_{p}\cdot(X_{desired} - X_{current})$$This controller is going to apply forces in the direction that decreases the error signal.
The robot state is given to you as $(x, y, \theta, \dot{x}, \dot{y}, \dot{th})$.
In [ ]:
import tutorial; reload(tutorial); from tutorial import *
initial_pose = (16, 12,0.0)
desired_pose = (16, 16,0.0)
K_px = 10
K_py = 10
K_pth = 10
def closedLoopController (time, robot_state):
# the output signal
x,y,th, xdot, ydot, thdot = robot_state
# the reference signal
rx, ry, rth = desired_pose
# the error signal
e_x = rx - x
e_y = ry - y
e_th = rth - th
# the controller output
u_x = K_px*e_x
u_y = K_py*e_y
u_th = K_pth*e_th
return u_x, u_y, u_th
result = run_pd_control(initial_pose, closedLoopController)
plot(result, "Robot")
Activities:
Try using different gains. See if you can observe different system response behavior, such as:
Improve upon your controller by adding a derivative term. In this case the reference signal for the derivative terms should be equal to 0.
`rxdot, rydot, rthdot = 0,0,0 `
In [ ]:
import tutorial; reload(tutorial); from tutorial import *
initial_pose = (16, 12,3.14/2)
desired_pose = (3, 16,0.0)
desired_vel = (0, 0, 0)
K_px = 100
K_py = 100
K_pth = 10
K_dx = 50
K_dy = 50
K_dth = 20
def closedLoopController (time, robot_state):
# the output signal
x,y,th, xdot, ydot, thdot = robot_state
# the reference signal
rx, ry, rth = desired_pose
rxdot, rydot, rthdot = desired_vel
# the error signal
e_x = rx - x
e_y = ry - y
e_th = rth - th
e_xdot = rxdot - xdot
e_ydot = rydot - ydot
e_thdot = rthdot - thdot
# the controller output
u_x = K_px*e_x + K_dx*e_xdot
u_y = K_py*e_y + K_dy*e_ydot
u_th = K_pth*e_th + K_dth*e_thdot
return u_x, u_y, u_th
result = run_pd_control(initial_pose, closedLoopController)
plot(result, "Robot")